#include <rthw.h>
#include <rtthread.h>

#include "../bsp/PRINT/my_printf.h"




rt_err_t rt_mp_init(rt_mp_t mp, const char *name, void *start, rt_size_t size, rt_size_t block_size){
    rt_ubase_t offset;

    rt_object_init(&(mp->parent), RT_Object_Class_MemPool, name); // 对象初始化

    mp->start_address = start;

    size = RT_ALIGN_DOWN(size, RT_ALIGN_SIZE);
    mp->size = size;

    block_size = RT_ALIGN(block_size, RT_ALIGN_SIZE);
    mp->block_size = block_size;

    mp->block_free_count = size / (block_size + sizeof(rt_ubase_t *));
    mp->block_total_count = mp->block_free_count;

    rt_list_init(&(mp->suspend_thread));
    mp->suspend_thread_count = 0;

    /* 形成单链表 */
    for(offset = 0; offset < mp->block_total_count - 1; offset++){
        *(rt_ubase_t **)((rt_uint8_t *)start + (block_size + sizeof(rt_ubase_t *)) * offset) = 
            (rt_ubase_t *)((rt_uint8_t *)start + (block_size + sizeof(rt_ubase_t *)) * (offset + 1));
    }
    *(rt_ubase_t **)((rt_uint8_t *)start + (block_size + sizeof(rt_ubase_t *)) * offset) = RT_NULL;

    mp->block_list = (rt_uint8_t *)start;

    return RT_EOK;
}

rt_err_t rt_mp_detach(rt_mp_t mp){
    rt_ubase_t tmp;
    rt_thread_t thread;

    RT_ASSERT(mp != RT_NULL);

    while(!rt_list_isempty(&(mp->suspend_thread))){
        tmp = rt_hw_interrupt_disable();

        thread = rt_list_entry(mp->suspend_thread.next, struct rt_thread, tlist);

        thread->error = -RT_ERROR;

        rt_thread_resume(thread);

        mp->suspend_thread_count--;

        rt_hw_interrupt_enable(tmp);
    }

    rt_object_detach(&(mp->parent));

    return RT_EOK;
}

rt_mp_t rt_mp_create(const char *name, rt_size_t block_count, rt_size_t block_size){
    rt_mp_t mp;
    rt_uint8_t *ptr;
    rt_size_t offset;

    mp = (rt_mp_t)rt_object_allocate(RT_Object_Class_MemPool, name);
    if(mp == RT_NULL){
        return RT_NULL;
    }

    block_size = RT_ALIGN(block_count, RT_ALIGN_SIZE);
    mp->block_size = block_size;

    mp->size = block_count * (block_size + sizeof(rt_uint32_t *));

    ptr = (rt_uint8_t *)rt_malloc(mp->size);
    if(ptr == RT_NULL){
        rt_object_detach(&mp->parent);

        return RT_NULL;
    }

    mp->start_address = (void *)ptr;
    mp->block_total_count = block_count;
    mp->block_free_count = block_count;

    rt_list_init(&mp->suspend_thread);
    mp->suspend_thread_count = 0;

    for(offset = 0; offset < block_count - 1; offset++){
        *(rt_uint8_t **)(ptr + offset * (block_size + sizeof(rt_uint32_t *))) = 
            (rt_uint8_t *)(ptr + (offset + 1) * (block_size + sizeof(rt_uint32_t *)));
    }
    *(rt_uint8_t **)(ptr + offset * (block_size + sizeof(rt_uint32_t *))) = RT_NULL;

    mp->block_list = ptr;

    return mp;
}

rt_err_t rt_mp_delete(rt_mp_t mp){
    rt_ubase_t tmp;
    rt_thread_t thread;

    RT_ASSERT(mp != RT_NULL);

    while(!rt_list_isempty(&mp->suspend_thread)){
        tmp = rt_hw_interrupt_disable();

        thread = rt_list_entry(mp->suspend_thread.next, struct rt_thread, tlist);

        rt_thread_resume(thread);

        mp->suspend_thread_count--;

        rt_hw_interrupt_enable(tmp);
    }

    rt_free(mp->start_address);

    rt_object_delete(&mp->parent);

    return RT_EOK;
}

void *rt_mp_alloc(rt_mp_t mp, rt_int32_t time){
    rt_thread_t thread;
    rt_uint8_t *block_ptr;
    rt_ubase_t tmp;
    rt_uint32_t before_sleep = 0;

    thread = rt_thread_self();

    tmp = rt_hw_interrupt_disable();

    while(mp->block_free_count == 0){
        if(time == 0){  // 无等待的直接返回
            thread->error = -RT_ETIMEOUT;
            rt_hw_interrupt_enable(tmp);

            return RT_NULL;
        }

        thread->error = RT_EOK;

        rt_thread_suspend(thread);  // 等待的挂起线程
        rt_list_insert_before(&(mp->suspend_thread), &(thread->tlist));
        mp->suspend_thread_count++;

        if(time > 0){
            before_sleep = rt_tick_get();

            rt_timer_control(&thread->thread_timer, RT_TIMER_CTRL_SET_TIME, &time);
            rt_timer_start(&thread->thread_timer);  // 启动定时器
        }

        rt_hw_interrupt_enable(tmp);

        rt_schedule();  // 调度

        /* 超时或主动恢复线程 */
        if(thread->error != RT_EOK){
            return RT_NULL;
        }

        if(time > 0){
            time = rt_tick_get() - before_sleep;    // 未分配内存块，且未超时继续等待
            if(time < 0){
                time = 0;
            }
        }

        tmp = rt_hw_interrupt_disable();
    }

    /* 执行到这说明有空闲块了 */
    block_ptr = mp->block_list;
    mp->block_list = *(rt_uint8_t **)block_ptr;     // .block_list总是指向下一个空闲块
    *(rt_uint8_t **)block_ptr = (rt_uint8_t *)mp;

    mp->block_free_count--;

    rt_hw_interrupt_enable(tmp);

    return (void *)(block_ptr + sizeof(rt_uint8_t *));
}

void rt_mp_free(void *block){
    rt_uint8_t *block_ptr;
    rt_mp_t mp;
    rt_thread_t thread;
    rt_ubase_t tmp;

    RT_ASSERT(block != RT_NULL);

    block_ptr = (rt_uint8_t *)((rt_uint32_t)block - sizeof(rt_uint8_t *));
    mp = (rt_mp_t)(*(rt_uint8_t **)block_ptr);  // 获取该内存池控制块

    tmp = rt_hw_interrupt_disable();

    *(rt_uint8_t **)block_ptr = (rt_uint8_t *)mp->block_list;   // .next
    mp->block_list = block_ptr; // 本块又变成了链表中的第一块空闲块

    mp->block_free_count++;

    if(mp->suspend_thread_count > 0){   // 判断是否有线程等待分配
        thread = rt_list_entry(mp->suspend_thread.next, struct rt_thread, tlist);
        rt_thread_resume(thread);

        thread->error = RT_EOK;

        mp->suspend_thread_count--;

        rt_hw_interrupt_enable(tmp);

        rt_schedule();

        return ;
    }

    rt_hw_interrupt_enable(tmp);
}
